// Adafruit Motor shield library
// copyright Adafruit Industries LLC, 2009
// this code is public domain, enjoy!
// Include the Stepper Library
#include <Stepper.h>

// Map our pins to constants to make things easier to keep track of
const int pwmA = 3;
const int pwmB = 11;
const int brakeA = 9;
const int brakeB = 8;
const int dirA = 12;
const int dirB = 13;

// The amount of steps for a full revolution of your motor.
// 360 / stepAngle
const int STEPS = 200;

// Initialize the Stepper class
Stepper myStepper(STEPS, dirA, dirB);

#include <AFMotor.h>


int location[7];
int currentLocation = 0;
int drink = 0;


void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Stepper test!");
  location[0] = 0;
  location[1] = 246;
  location[2] = 512;
  location[3] = 780;
  location[4] = 1037;
  location[5] = 1292;
  location[6] = 1567;
  pinMode(2, OUTPUT);
  pinMode(4, INPUT);
  pinMode(5, INPUT);
  pinMode(6, INPUT);
  pinMode(7, INPUT);
  
   // Set the RPM of the motor
  myStepper.setSpeed(65);

  // Turn on pulse width modulation
  pinMode(pwmA, OUTPUT);
  digitalWrite(pwmA, HIGH);
  pinMode(pwmB, OUTPUT);
  digitalWrite(pwmB, HIGH);

  // Turn off the brakes
  pinMode(brakeA, OUTPUT);
  digitalWrite(brakeA, LOW);
  pinMode(brakeB, OUTPUT);
  digitalWrite(brakeB, LOW);
  
  myStepper.setSpeed(65);  // 10 rpm   
}

void loop() {
  
  for(int i = 0; i < 20; i++){
    if(digitalRead(i) == HIGH)
      Serial.println(i);
  }
  
  if(digitalRead(4)==HIGH){
    drink1();
  }
  
  if(digitalRead(5)== HIGH){
    drink2();
  }
  if(digitalRead(6)==HIGH){
   drink3(); 
  }
   
}

void goTo(int x){
  int newLocation = location[x];
  int movement = newLocation - currentLocation;
  
  if(movement > 0){
  myStepper.step(movement*1.008); 
  }
  else{
  myStepper.step(movement); 
  }
  currentLocation = newLocation;
}

void getShot(){
  digitalWrite(2,HIGH);
  delay(9500);
  digitalWrite(2,LOW);
}


void drink1(){
  goTo(1);
  getShot();
  getShot();
  goTo(4);
  getShot();
  goTo(0);
}

void drink2(){
  goTo(3);
  getShot();
  goTo(5);
  getShot();
  goTo(6);
  getShot();
  goTo(0);
}

void drink3(){
  goTo(5);
  getShot();
  getShot();
  goTo(2);
  getShot();
  goTo(1);
  getShot();
  goTo(0);
}
